#include "./door_motor.h"
#include "../device/gpio.h"
#include "./analog.h"
#include "./mux.h"
#include "../utils/progress.h"

namespace driver {

using namespace device::gpio;
using namespace utils;

using DIR1 = PC<12>;
using DIR2 = PC<11>;
using EN = PC<10>;

enum class Dir : uint8_t {

    OPEN = 0,
    CLOSE = 1,
    BRAKE = 2,
};

static void set_dir(Dir dir) {
    switch (dir) {
        case Dir::OPEN:
            DIR1::clr();
            DIR2::set();
            EN::clr();
        break;

        case Dir::CLOSE:
            DIR1::set();
            DIR2::clr();
            EN::clr();
        break;

        case Dir::BRAKE:
            DIR1::set();
            DIR2::set();
            EN::set();
        break;
    }
}

DoorMotor::DoorMotor() {

    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;

    config_pp_out<DIR1, DIR2, EN>();
    set_dir(Dir::BRAKE);
}

Error DoorMotor::open() {
    Error err;
    Progress prog;
    uint8_t lock_count = 0;

    if (mux->is_door_front()) {
        goto finish;
    }   

    set_dir(Dir::OPEN);
    for (;;) {

        osDelay(5);

        if (mux->is_door_front()) {
            break;
        }

        if (prog.is_after(TIMEOUT)) {
            err = ec::DOOR_MOTOR_TIMEOUT;
            break;
        }

        if (prog.is_after(500) && analog->is_door_locked()) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::DOOR_MOTOR_LOCKED;
                break;
            }
        } else {
            lock_count = 0;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    return err;
}

Error DoorMotor::close() {
    Error err;
    Progress prog;
    uint8_t lock_count = 0;

    if (mux->is_door_back()) {
        goto finish;
    }   
    set_dir(Dir::CLOSE);
    for (;;) {

        osDelay(5);

        if (mux->is_door_back()) {
            break;
        }

        if (prog.is_after(TIMEOUT)) {
            err = ec::DOOR_MOTOR_TIMEOUT;
            break;
        }

        if (prog.is_after(500) && analog->is_door_locked()) {
            lock_count ++;
            if (lock_count >= 5) {
                set_dir(Dir::BRAKE);
                err = ec::DOOR_MOTOR_LOCKED;
                osDelay(500);
                open();
                break;
            }
        } else {
            lock_count = 0;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    return err;
}

}
